
#include <stdio.h>

#include "typedfs.h"
#include "img_tool.h"


/***********  FOR MORPHOLOGY ***********/


/** In place right shift R1:=R1 shr 1 */
void ShrR(void *R, unsigned maxX)
{
uint8_t PrevVal, val;
  if(R==NULL || maxX==0) return;

  PrevVal = (*(uint8_t*)R & 0x80);
  while(maxX-->0)
  {
    val = *(uint8_t*)R;
    *(uint8_t*)R = PrevVal | (val>>1);
    R = (uint8_t*)R + 1;
    PrevVal = (val & 1) ? 0x80 : 0;
  }
}


/** In place left shift R1:=R1 shl 1 */
void ShlR(void *R, unsigned maxX, unsigned char LastBit)
{
uint8_t PrevVal, val;
  if(R==NULL || maxX==0) return;

  R = (int8_t*)R + maxX - 1;

  PrevVal = *(uint8_t*)R & LastBit;
  while(maxX-->0)
  {
    val = *(uint8_t*)R;
   *(int8_t*)R = PrevVal | (val<<1);
   R = (uint8_t*)R - 1;
   PrevVal = (val & 0x80) ? 1 : 0;
  }
}


/** Find a maximum from two BYTE vectors: R1 = max(R1,R2).
 * @param[in,out] R1	First vector and result on return.
 * @param[in]	  R2	Second vector.
 * @param[in]	  size	Amount of bytes. */
void MaxU8(unsigned char *R1, const unsigned char *R2, unsigned size)
{
  while(size-->0)
  {
    if(*R2 > *R1) *R1=*R2;
    R1++;
    R2++;
  }
}


/** Find a minumum from two BYTE vectors: R1 = min(R1,R2).
 * @param[in,out] R1	First vector and result on return.
 * @param[in]	  R2	Second vector.
 * @param[in]	  size	Amount of bytes. */
void MinU8(unsigned char *R1, const unsigned char *R2, unsigned size)
{
  while(size-->0)
  {
    if(*R2 < *R1) *R1=*R2;
    R1++;
    R2++;
  }
}


void MaxU16(unsigned char *R1, const unsigned char *R2, unsigned size)
{
  while(size-->0)
  {
    if(*(uint16_t*)R2 > *(uint16_t*)R1) *(uint16_t*)R1=*(uint16_t*)R2;
    R1+=2;
    R2+=2;
  }
}


/** Find a minumum from two WORD vectors: R1 = min(R1,R2) */
void MinU16(unsigned char *R1, const unsigned char *R2, unsigned size)
{
  while(size-->0)
  {
    if(*(uint16_t*)R2 < *(uint16_t*)R1) *(uint16_t*)R1=*(uint16_t*)R2;
    R1+=2;
    R2+=2;
  }
}


/* -----------------------*/
#define STORE_RGB8_FROM_YUV10 X = (Y + 291*V) >> 10; /* Y + 1.137*V; */ \
   if(X>255) *out++=255;\
   else if(X<0) *out++=0;\
   else *out++ = X;\
   X = (Y -102*U - 148*V) >> 10; /* Y - 0.397*U - 0.58*V;*/ \
   if(X>255) *out++=255;\
   else if(X<0) *out++=0;\
   else *out++ = X;\
   X = (Y + 521*U) >> 10;	/* Y + 2.034*U;*/ \
   if(X>255) *out++=255;\
   else if(X<0) *out++=0;\
   else *out++ = X;


void V210_RGB(uint8_t *out, const uint32_t *in, int LoopCount)
{
int32_t X, Y;
int16_t U;
int16_t V;

 while(LoopCount-->0)
 {
   U = (0x3FF & *in) - 0x200;		// block 1, bits  0 -  9: U0+1
   Y = 0x3FF00 & (*in>>2);		// block 1, bits 10 - 19: Y0
   V = (0x3FF & (*in>>20)) - 0x200;	// block 1, bits 20 - 29: V0+1

   STORE_RGB8_FROM_YUV10
  
   if(LoopCount--<=0) break;
   in++;
   Y = (0x3FF & *in)<<8;		// block 2, bits  0 -  9: Y1
   STORE_RGB8_FROM_YUV10

   if(LoopCount--<=0) break;  
   U = (0x3FF&(*in>>10)) - 0x200;	// block 2, bits 10 - 19: U2+3
   Y = (0x3FF00&(*in>>12));		// block 2, bits 20 - 29: Y2
   in++;
   V = (0x3FF&*in) - 0x200;		// block 3, bits  0 -  9: V2+3
   STORE_RGB8_FROM_YUV10

   if(LoopCount--<=0) break;
   Y = 0x3FF00&(*in>>2);		// block 3, bits 10 - 19: Y3
   STORE_RGB8_FROM_YUV10

   if(LoopCount--<=0) break;
   U = (0x3FF&(*in>>20)) - 0x200;	// block 3, bits 20 - 29: U4+5
   in++;
   Y = 0x3FF00 & (*in<<8);		// block 4, bits  0 -  9: Y4
   V = (0x3FF & (*in>>10)) - 0x200;	// block 4, bits 10 - 19: V4+5
   STORE_RGB8_FROM_YUV10

   if(LoopCount--<=0) break;
   Y = 0x3FF00 & (*in>>12);		// block 4, bits 20 - 29: Y5
   STORE_RGB8_FROM_YUV10

   in++;
 }
}


void V210_Gray24(uint8_t *out, const uint32_t *in, int LoopCount)
{
 while(LoopCount-->0)
 {    
   out[0]=out[1]=out[2] = 0xFF&(*in>>12);	// block 1, bits 10 - 19: Y0
  
   if(LoopCount--<=0) break;
   out+=3;
   in++;   
   out[0]=out[1]=out[2] = 0xFF&(*in >> 2);	// block 2, bits  0 -  9: Y1

   if(LoopCount--<=0) break;
   out+=3;   
   out[0]=out[1]=out[2] = 0xFF & (*in>>22);	// block 2, bits 20 - 29: Y2

   if(LoopCount--<=0) break;
   out+=3;
   in++;
   out[0]=out[1]=out[2] = 0xFF & (*in>>12);	// block 3, bits 10 - 19: Y3

   if(LoopCount--<=0) break;
   out+=3;
   in++;
   out[0]=out[1]=out[2] = 0xFF & (*in >> 2);	// block 4, bits  0 -  9: Y4

   if(LoopCount--<=0) break;
   out+=3;
   out[0]=out[1]=out[2] = 0xFF & (*in >> 22);	// block 4, bits 20 - 29: Y5

   in++;
   out+=3;
 }
}


void V210_Gray8(uint8_t *out, const uint32_t *in, int LoopCount)
{
 while(LoopCount-->0)
 {    
   *out++ = 0xFF&(*in>>12);	// block 1, bits 10 - 19: Y0
  
   if(LoopCount--<=0) break;   
   in++;   
   *out++ = 0xFF&(*in >> 2);	// block 2, bits  0 -  9: Y1

   if(LoopCount--<=0) break;
   *out++ = 0xFF & (*in>>22);	// block 2, bits 20 - 29: Y2

   if(LoopCount--<=0) break;
   in++;
   *out++ = 0xFF & (*in>>12);	// block 3, bits 10 - 19: Y3

   if(LoopCount--<=0) break;
   in++;
   *out++ = 0xFF & (*in >> 2);	// block 4, bits  0 -  9: Y4

   if(LoopCount--<=0) break;
   *out++ = (*in >> 22);	// block 4, bits 20 - 29: Y5

   in++;
 }
}


void UYVY_RGB(unsigned char *OutData, const unsigned char *yuyv, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)
  {
    const signed char U = *yuyv++ - 128;
    const int32_t Y = 256 * *(yuyv++);
    const signed char V = *yuyv++ - 128;
    const int32_t Y2 = 256 * *(yuyv++);

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;

    X = (Y2 + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y2 -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y2 + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;    
    PixelCount -= 2;
  }
}


void Y_UV_RGB(unsigned char *OutData, const unsigned char *y, const unsigned char *uv, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)   //converts to RGB
  {
    const signed char U = *uv++ - 128;
    const signed char V = *uv++ - 128;
    int32_t Y = 256 * *(y++);

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;
    Y = 256 * *(y++);

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;
    PixelCount -= 2;
  }
}


/***********  FOR GAUSIAN ***********/


/// Calculate gausian for 1D vector.
void Gaus1D_u32(uint32_t *line, int sizex, uint16_t times)
{
uint32_t A1, A2;
unsigned x;

  if(line==NULL || sizex<=1) return;
  sizex--;

  while(times-->0)
  {
    A1 = *line / 2;
    for(x=0; x<sizex; x++)
    {
      A2 = line[x]/4 + line[x+1]/4;
      line[x] = A1 + A2;
      A1 = A2;
    }
    line[sizex] = (A1 + line[sizex]/2);
  }
}


void GausVer1_u32(uint32_t *Out, const uint32_t *L1, const uint32_t *L2, const uint32_t *L3 , int sizex)
{
  while(sizex-- > 0)
  {
    *Out++ = *L1/4 + *L2/2 + *L3/4;
    L1++; L2++; L3++;
  }
}


/// Calculate difference inside 1 line.
/// param[out]	Out	Output
/// param[in]	In	1st line. It is tollerated Out==In.
void DiffHor1_u32(uint32_t *Out, const uint32_t *In , int SizeX)
{
uint32_t Val, Val2;

  if(Out==NULL || In==NULL) return;

  switch(SizeX)
  {
    case 0: return;
    case 1: *Out=0x80000000; return;
  }

  Val = *In/2;
  while(SizeX-->=2)
  {
    Val2 = (*In++)/2;		/* Pointers In and Out could be the same, prevent overwrite. */
    *Out++ = (*In/2+0x80000000) - Val;
    Val = Val2;
  }
  *Out = (*In/2+0x80000000) - Val;
}


void AbsCopy_u32(uint32_t *Out, uint32_t *In1, uint32_t *In2, int SizeX)
{
  while(SizeX-- > 0)
  {
    if(*In1 > *In2)
      *Out++ = *In1++ - *In2++;
    else
      *Out++ = *In2++ - *In1++;
  }
}


void AbsHor_u32(uint32_t *Out, uint32_t *In, int SizeX)
{
  while(SizeX-- > 0)
  {
    if(*In > *Out)
      *Out++ = *In++ - *Out;
    else
      *Out++ = *Out - *In++;
  }
}
